/*
 * Created on 16 f�vr. 2005
 *
 * TODO To change the template for this generated file go to
 * Window - Preferences - Java - Code Style - Code Templates
 */

package picoevo.tutorials.simbad.robotavoidbehavior;

import java.util.ArrayList;

import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;
import javax.vecmath.Vector3f;

import picoevo.toolbox.Display;
import piconode.core.node.FeedForwardNeuralNetwork;
import piconode.factory.MultiLayerPerceptronFactory;
import simbad.gui.Simbatch;
import simbad.sim.Agent;
import simbad.sim.Box;
import simbad.sim.EnvironmentDescription;
import simbad.sim.RangeSensorBelt;
import simbad.sim.RobotFactory;
import simbad.sim.Wall;

/**
 * Test of the batch mode - test Simbatch class.
 */
public class Evaluator_SimpleAvoiderRobot extends EnvironmentDescription {

	public double _fitness = 0;
	private double[] _genome;
	public boolean _isRunnable = true;
	MyRobot _myRobot = new MyRobot(new Vector3d(+4, 0, +3.5f), "picoevorobot example 1", this);

	public Evaluator_SimpleAvoiderRobot() {
		// build the environment
		Wall w1 = new Wall(new Vector3d(9, 0, 0), 19, 1, this);
		w1.rotate90(1);
		add(w1);
		Wall w2 = new Wall(new Vector3d(-9, 0, 0), 19, 2, this);
		w2.rotate90(1);
		add(w2);
		Wall w3 = new Wall(new Vector3d(0, 0, 9), 19, 1, this);
		add(w3);
		Wall w4 = new Wall(new Vector3d(0, 0, -9), 19, 2, this);
		add(w4);

		// add(new Box(new Vector3d(-5,0,0),new Vector3f(0.1f,1,10),this));
		// add(new Box(new Vector3d(0,0,-5),new Vector3f(10,1,0.1f),this));
		// add(new Box(new Vector3d(5,0,0),new Vector3f(0.1f,1,10),this));
		add(new Box(new Vector3d(5, 0, 0), new Vector3f(1f, 1, 10), this));
		add(new Box(new Vector3d(0, 0, 5), new Vector3f(10, 1, 1f), this));
		add(new Box(new Vector3d(0, 0, 0), new Vector3f(4, 1, 4), this));

		// create the robot and record the NN weights
		add(_myRobot);
	}

	public void setGenome(double[] __neuralNetWeightsList) {
		this._genome = __neuralNetWeightsList;
	}

	public class MyRobot extends Agent {

		// RangeSensorBelt bumpers;
		RangeSensorBelt sonars;

		private Evaluator_SimpleAvoiderRobot _setup; // useless... unless we
		// later decide to
		// externalize this
		// internal class (!n)
		private FeedForwardNeuralNetwork _robotNetworkController;

		ArrayList _inputControllerValuesList;

		public MyRobot(Vector3d position, String name, Evaluator_SimpleAvoiderRobot __setup) {
			super(position, name);

			_setup = __setup;

			// Add sensors
			// bumpers = RobotFactory.addBumperBeltSensor(this, 12);
			sonars = RobotFactory.addSonarBeltSensor(this, 12);

			_inputControllerValuesList = new ArrayList();
			/*
			 * _inputControllerValuesList.add(new Double(0));
			 * _inputControllerValuesList.add(new Double(0));
			 * _inputControllerValuesList.add(new Double(0));
			 * _inputControllerValuesList.add(new Double(0));
			 */

		}

		/**
		 * Initialize Agent's Behavior -- launched before the first
		 * performOneStep
		 */
		@Override
		public void initBehavior() {
			this._robotNetworkController = MultiLayerPerceptronFactory.createPerceptron(4, 4, 2, false);
			// this._robotNetworkController.displayInformation();
		}

		/** move robot at start position and load the new genome */
		public void resetEvaluation() {

			this.moveToStartPosition();

			ArrayList myGenomeList = new ArrayList();
			for (int i = 0; i != this._setup._genome.length; i++)
				myGenomeList.add(new Double(_setup._genome[i]));

			// System.out.print(" taille : " + myGenomeList.size());
			// System.exit(-1);
			this._robotNetworkController.setAllArcsWeightValues(myGenomeList);

			this._setup._fitness = 0;
			this._setup._isRunnable = true;
		}

		@Override
		public void performBehavior() {

			// Computing the fitness
			// step 1 of 2
			// updating the fitness for the previous step and checking for
			// collision
			if (collisionDetected() == true) {
				_setup._fitness = 0;
				// Display.info("collision!");
				this._setup._isRunnable = false; // if collision : stop
				// simulation session
				return;
			}
			// else
			// _setup._fitness = _setup._fitness + 1;

			// every 20 frames
			if (getCounter() % 20 == 0) {

				// * reads the sonar values

				double[] inputValue = new double[4];

				// inputValue[0] = ( sonars.getMeasurement (0) +
				// sonars.getMeasurement (1) + sonars.getMeasurement (11) +
				// sonars.getMeasurement (10) ) / 4;
				// inputValue[1] = ( sonars.getMeasurement (2) +
				// sonars.getMeasurement (3) ) / 2;
				// inputValue[2] = ( sonars.getMeasurement (8) +
				// sonars.getMeasurement (9) ) / 2;
				// inputValue[3] = ( sonars.getMeasurement (5) +
				// sonars.getMeasurement (6) ) / 2;

				inputValue[0] = sonars.getFrontQuadrantMeasurement();
				inputValue[1] = sonars.getFrontRightQuadrantMeasurement();
				inputValue[2] = sonars.getFrontLeftQuadrantMeasurement();
				inputValue[3] = 0;

				for (int i = 0; i != 4; i++) {
					if (inputValue[i] == Double.POSITIVE_INFINITY) {
						inputValue[i] = 1.5;
					}
					// System.out.println(" quadrantSonar("+i+") =
					// "+inputValue[i]);
				}

				// * load the new input values into the controller
				_inputControllerValuesList.clear();
				for (int i = 0; i != 4; i++) {
					_inputControllerValuesList.add(new Double(inputValue[i]));
				}
				this._robotNetworkController.step(_inputControllerValuesList);
				// this._robotNetworkController.displayInformation();

				// * perform locomotion step
				double speed = _robotNetworkController.getOutputNeuronAt(0).getValue();
				double angle = _robotNetworkController.getOutputNeuronAt(1).getValue();
				setTranslationalVelocity(speed);
				setRotationalVelocity(angle);
				Point3d coord = new Point3d();
				this.getCoords(coord);
				// System.out.println("[time:"+ getCounter() + "] " + "( " +
				// coord.x + " , " + coord.y + " ) ; " + "commande ( "+
				// _robotNetworkController.getOutputNeuronAt(0).getNeuronValue()
				// + " , " + vitesse + " )");

				// Computing the fitness
				// step 2 of 2
				// penalize if not moving
				double maxSensorValue = 0;
				for (int i = 0; i != 4; i++)
					if (maxSensorValue < inputValue[i])
						maxSensorValue = inputValue[i];
				_setup._fitness += speed + (1 - angle) + (1 - maxSensorValue); // adapted
				// classic
				// fitness
				// from
				// Nolfi&Floreano2000EvolutionaryRobotics,
				// p73-74
			}
		}
	}

	double computeRobotFitnessWithLightProcessing(Simbatch sim, double genome[]) {
		this._myRobot.resetEvaluation();

		int i = 0;
		while (i < 10000 && _isRunnable == true) {
			sim.step();
			i++;
		}

		Display.info("fitness is " + _fitness + "");

		return (_fitness);
	}

	static double computeRobotFitnessTest(double genome[]) {

		Evaluator_SimpleAvoiderRobot ers = new Evaluator_SimpleAvoiderRobot();

		Simbatch sim = new Simbatch(ers, true);
		sim.reset();

		int i = 0;
		while (i < 10000 && ers._isRunnable == true) {
			try {
				Thread.sleep(10);
			} catch (Exception e) {
			} // [!n] hack : make it visible...
			sim.step();
			i++;
		}

		Display.info("fitness is " + ers._fitness + "");
		sim.dispose();

		return (ers._fitness);
	}

	// ----------- demo method

	// return the fitness for a given genome
	public static double computeFitness(double[] genome) {
		Evaluator_SimpleAvoiderRobot robot = new Evaluator_SimpleAvoiderRobot();
		Simbatch simulator = new Simbatch(robot, true);

		simulator.reset();
		robot.setGenome(genome);
		robot._myRobot.resetEvaluation();

		int i = 0;
		while (i < 10000 && robot._isRunnable == true) {
			simulator.step();
			i++;
		}

		Display.info("fitness is " + robot._fitness + "");
		Display.info("with genome : ");
		for (int j = 0; j < genome.length; j++)
			System.out.print(genome[j] + " ; ");
		Display.info("");

		return (robot._fitness);
	}

	public static void main(String[] args) {

		// The following genome is for example purpose (it does not actually
		// fulfill the task - random genome).

		double genome[] = { -0.5687399872869805, -0.9554699688080246, 0.4772563411155608, -0.7202728546127957, 0.4119730929168963, -0.8068253131237191, -0.3643513176764104, 0.8781821410043917, -0.23512862677415036, -0.7041303862298087, -0.4113173429099277, -0.7317029043160093, 0.3012876529788864, 0.22053091556991355, -0.906677183737588, -0.7316144958520516, 0.14735554569250597, 0.18971519217624322, -0.35501010222316576, -0.09035628814973728, 0.12053614616318664, -0.46431027781496215,
				-0.979288920263091, 0.9754937292326014, 0.8142841918296084, 0.245554205850127, -0.46431027781496215, -0.979288920263091, 0.9754937292326014, 0.8142841918296084, 0.245554205850127 };
		/*
		 * double genome [27]; for ( int i = 0 ; i != genome.length ; i++ )
		 * genome[i] = Math.random() * 2 - 1;
		 */
		for (int i = 0; i != 10; i++)
			computeRobotFitnessTest(genome);
	}
}
